# transformations.py
#
# Functions to deal with transformation matrices (homogeneous)

import math
import numpy

import unittest

def identityTransformationArray():
    ident = [[1.0, 0.0, 0.0, 0.0],
             [0.0, 1.0, 0.0, 0.0],
             [0.0, 0.0, 1.0, 0.0],
             [0.0, 0.0, 0.0, 1.0]]
    return ident

def identityTransformationMatrix():
    return numpy.matrix(identityTransformationArray())

def givensRotation(i, j, theta, degrees=False):
    if degrees is True:
        theta = math.radians(theta)
    givens = identityTransformationArray()
    c = math.cos(theta)
    s = math.sin(theta)
    givens[i][i] = c
    givens[j][j] = c
    givens[i][j] = s
    givens[j][i] = -s
    return numpy.matrix(givens)

def xRotationMatrix(theta, degrees=False, right_handed=True):
    if right_handed is True:
        theta = -theta
    return givensRotation(1, 2, theta, degrees)

def yRotationMatrix(theta, degrees=False, right_handed=True):
    if right_handed is False:
        theta = -theta
    return givensRotation(0, 2, theta, degrees)

def zRotationMatrix(theta, degrees=False, right_handed=True):
    if right_handed is True:
        theta = -theta
    return givensRotation(0, 1, theta, degrees)

def translationMatrix(x=0.0, y=0.0, z=0.0):
    t = identityTransformationArray()
    t[0][3] = x
    t[1][3] = y
    t[2][3] = z
    return numpy.matrix(t)

def scaleMatrix(x=1.0, y=1.0, z=1.0):
    s = identityTransformationArray()
    s[0][0] = x
    s[1][1] = y
    s[2][2] = z
    return numpy.matrix(s)


#def shear(?): # TODO: what's a shear matrix?
    
if __name__ == '__main__':
    class TestTransformations(unittest.TestCase):

        def test_identity(self):
            identity = identityTransformationMatrix()
            for row in range(4):
                for col in range(4):
                    if row == col:
                        self.assert_(identity.item(row, col) == 1.0)
                    else:
                        self.assert_(identity.item(row, col) == 0.0)

        def test_rotation(self):
            theta_deg = 90
            theta = math.radians(theta_deg)
            xRot = xRotationMatrix(theta)
            self.assert_(xRot.item(1, 1) == math.cos(-theta))
            self.assert_(xRot.item(2, 2) == math.cos(-theta))
            self.assert_(xRot.item(1, 2) == math.sin(-theta))
            self.assert_(xRot.item(2, 1) == -math.sin(-theta))
            yRot = yRotationMatrix(theta_deg, True)
            self.assert_(yRot.item(0, 0) == math.cos(theta))
            self.assert_(yRot.item(2, 2) == math.cos(theta))
            self.assert_(yRot.item(0, 2) == math.sin(theta))
            self.assert_(yRot.item(2, 0) == -math.sin(theta))
            zRot = zRotationMatrix(theta, False, False)
            self.assert_(zRot.item(0, 0) == math.cos(theta))
            self.assert_(zRot.item(1, 1) == math.cos(theta))
            self.assert_(zRot.item(0, 1) == math.sin(theta))
            self.assert_(zRot.item(1, 0) == -math.sin(theta))
        
        def test_translate(self):
            x = -110.5
            y = 10
            z = 22.703
            t = translationMatrix(x, y, z)
            self.assert_(t.item(0, 3) == x)
            self.assert_(t.item(1, 3) == y)
            self.assert_(t.item(2, 3) == z)

        def test_scale(self):
            x = 4.1892
            y = 99.99
            z = 1.00
            s = scaleMatrix(x, y, z)
            self.assert_(s.item(0, 0) == x)
            self.assert_(s.item(1, 1) == y)
            self.assert_(s.item(2, 2) == z)
            
    unittest.main()
